/**
 * @file ebex_model.c
 *
 * @date Oct 25, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <memory_util.h>
#include <log_util.h>

#include <ebex_quaternion.h>
#include <data_file.h>

#include <ebex_model.h>

ebex_model_t *ebex_model_new(size_t m_gyroboxes, size_t m_starcameras, size_t m_segments)
{
	ebex_model_t *retval;

	ebex_quat_t sc_variance;

	retval = e_calloc(1,sizeof(ebex_model_t));
	retval->num_sc = m_starcameras;
	retval->num_gyroboxes = m_gyroboxes;

	retval->data = e_calloc(1,sizeof(ebex_gyro_sc_t));

	retval->data->gyrobox_source = e_calloc(m_gyroboxes, sizeof(data_source_t*));
	retval->data->sc_source = e_calloc(m_starcameras, sizeof(data_source_t*));
	retval->gyrobox = e_calloc(m_gyroboxes, sizeof(gyrobox_t));
	retval->sc = e_calloc(m_starcameras, sizeof(sc_param_t));

	retval->err_quats = e_calloc(m_segments, sizeof(ebex_quat_t));
	retval->err_weights = e_calloc(m_segments, sizeof(double));

	for (size_t i = 0; i < m_starcameras; i++)
	{
		ebex_quat_from_euler(&sc_variance, &filter_config.sc[i].measurement_variance_rpy, rotation_order_XYZ);
		retval->sc[i].measurement_variance = pow(2.0 * acos(sc_variance.w), 2.0);
	}
	return retval;
}


